#include "FLOAT.h"
#include <stdint.h>
#include <assert.h>

FLOAT F_mul_F(FLOAT a, FLOAT b)
{
  FLOAT x1 = a & 0xFFFF;
  FLOAT x2 = b & 0xFFFF;
  FLOAT y1 = a >> 16;
  FLOAT y2 = b >> 16;
  FLOAT r = (y1 * y2) << 16;
  r += (x1 * y2 + x2 * y1);
  r += (x1 * x2) >> 16;
  return r;
}

FLOAT F_div_F(FLOAT a, FLOAT b)
{
  int op = 1;
  if (a < 0)
  {
    op = -op;
    a = -a;
  }
  if (b < 0)
  {
    op = -op;
    b = -b;
  }
  int ret = a / b;
  a %= b;
  int i;
  for (i = 0; i < 16; i++)
  {
    a <<= 1;
    ret <<= 1;
    if (a >= b)
      a -= b, ret |= 1;
  }
  return op * ret;
}

FLOAT f2F(float a)
{
  union float_ {
    struct
    {
      uint32_t m : 23;
      uint32_t e : 8;
      uint32_t signal : 1;
    };
    uint32_t value;
  };
  union float_ f;
  f.value = *((uint32_t *)(void *)&a);
  int e = f.e - 127;
  FLOAT result;
  if (e <= 7)
  {
    result = (f.m | (1 << 23)) >> 7 - e;
  }
  else
  {
    result = (f.m | (1 << 23)) << (e - 7);
  }
  return f.signal == 0 ? result : (result | (1 << 31));
}

FLOAT Fabs(FLOAT a)
{
  if (a < 0)
    return -a;
  else
    return a;
}

/* Functions below are already implemented */

FLOAT Fsqrt(FLOAT x)
{
  FLOAT dt, t = int2F(2);
  do
  {
    dt = F_div_int((F_div_F(x, t) - t), 2);
    t += dt;
  } while (Fabs(dt) > f2F(1e-4));
  return t;
}

FLOAT Fpow(FLOAT x, FLOAT y)
{
  /* we only compute x^0.333 */
  FLOAT t2, dt, t = int2F(2);
  do
  {
    t2 = F_mul_F(t, t);
    dt = (F_div_F(x, t2) - t) / 3;
    t += dt;
  } while (Fabs(dt) > f2F(1e-4));
  return t;
}
